/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "S_PhysicsBullet.h"

#include <ppl.h>
#include <list>

#include "物体/S_物体.h"


static std::vector<btVector3> g车辆检测射线(10);




S_PhysicsBullet::S_PhysicsBullet() {
	
	vec3 d = { 0.0f,0.0f,-20.0f };
	for (uint8 i = 0; i < 8; ++i) {
		//C_Mat44 mat = C_Mat44::kIdentity;
		C_Mat44 mat = RotationMatrix(M_角度转弧度(45.0 * i), {0.0f,1.0f,0.0});

		vec3 r = mat * d;
		g车辆检测射线[i] = {r.x, r.y, r.z};
	}

	C_Mat44 mat = RotationMatrix(M_角度转弧度(20.0), { 0.0f,1.0f,0.0 });
	vec3 r = mat * d;
	g车辆检测射线[8] = { r.x, r.y, r.z };
	mat = RotationMatrix(M_角度转弧度(-20.0), { 0.0f,1.0f,0.0 });
	r = mat * d;
	g车辆检测射线[9] = { r.x, r.y, r.z };
}

S_PhysicsBullet::~S_PhysicsBullet() {

}

void S_PhysicsBullet::f_Update() {
	uint16 num	= m_Core.m_物理域空间.size();
	auto* world	= m_Core.m_物理域空间.data();
	//return;

	if (mf_车辆操作更新) {

#ifdef _DEBUG
		for (auto& 车 : m_车) {
			mf_车辆操作更新(车);
		}
#else
		Concurrency::parallel_for_each(m_车.begin(), m_车.end(), [&](S_Bullet车* 车) {
			mf_车辆操作更新(车);
		});
		//for (auto& 车 : m_车) {
		//	mf_车辆操作更新(车);
		//}
#endif // DEBUG

		
	}
	
	


	for (uint32 i = 0; i < num; ++i) {
		//Concurrency::parallel_for<uint16>(0, num, [&](uint16 i) {
		world[i]->stepSimulation(m_步进时间, 3);

		if (world[i]->getConstraintSolver()->getSolverType() == BT_MLCP_SOLVER)
		{
			btMLCPSolver* sol = (btMLCPSolver*)world[i]->getConstraintSolver();
			int numFallbacks = sol->getNumFallbacks();
			if (numFallbacks) {
				static int totalFailures = 0;
				totalFailures += numFallbacks;
				//printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n", totalFailures);
			}
			sol->setNumFallbacks(0);
		}
		//});
	}
}

void S_PhysicsBullet::f_应用物理变换() {
	uint16 num = m_Core.m_物理域空间.size();
	auto* world = m_Core.m_物理域空间.data();

	//return;

	//print positions of all objects
	for (uint32 i = 0; i < num; ++i) {
		auto& 空间 = world[i];

		//Concurrency::parallel_invoke(
			//[&] {
				uint32 objNum = 空间->getNumCollisionObjects();

				for (uint32 j = 0; j < objNum; ++j) {
					btCollisionObject* obj = 空间->getCollisionObjectArray()[j];
					btRigidBody* body = btRigidBody::upcast(obj);
					btTransform trans;

					if (body && body->getMotionState()) {
						body->getMotionState()->getWorldTransform(trans);
					}
					else {
						trans = obj->getWorldTransform();
					}
					//printf("world pos object %d = %f,%f,%f\n", j, float(trans.getOrigin().getX()), float(trans.getOrigin().getY()), float(trans.getOrigin().getZ()));
					//trans.getOpenGLMatrix();

					S_物体* user = (S_物体*)body->getUserPointer();
					if (user) {
						Mat44f mat;
						trans.getOpenGLMatrix((float32*)&mat);
						user->f_set矩阵(&mat);
					}

				}
			//},




			//[&] {
				for (auto& 车 : m_车) {
					//Concurrency::parallel_for_each(m_车.begin(), m_车.end(), [&](S_Bullet车* 车) {
					for (uint32 w = 0; w < 4; ++w) {
						车->m_vehicle->updateWheelTransform(w, true);

						Mat44f mat;
						车->m_vehicle->getWheelTransformWS(w).getOpenGLMatrix((float32*)&mat);
						//btTransform trans = 车->m_vehicle->getWheelInfo(w).m_worldTransform;
						//trans.getOpenGLMatrix((float32*)&mat);
						((S_物体*)车->m_车轮对象[w])->f_set矩阵(&mat);
					}
					//});


					auto pos = 车->m_vehicle->getChassisWorldTransform().getOrigin();

//#ifdef _DEBUG
					for (uint8 r = 0; r < 10; ++r) {
//#else
//					Concurrency::parallel_for<uint16>(0, 8, [&](uint16 r) {
//#endif // DEBUG
						btVector3 to = pos + g车辆检测射线[r];
						//from + to;
						btCollisionWorld::ClosestRayResultCallback closestResults(pos, to);
						closestResults.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;

						空间->rayTest(pos, to, closestResults);
						if (closestResults.hasHit()) {
							btVector3 co = pos.lerp(to, closestResults.m_closestHitFraction);
							车->m_射线碰撞点[r] = vec3{ float32(co.x()), float32(co.y()), float32(co.z()) };
						}
						else {
							车->m_射线碰撞点[r] = vec3{ float32(to.x()), float32(to.y()), float32(to.z()) };
						}
//#ifdef _DEBUG
					}
//#else
//					});
//#endif // DEBUG
					
				}
			//},



				
			/************************************************************************************
			*                         全局碰撞回调
			*
			*************************************************************************************/
			//[&] {
				if (m_全局碰撞回调) {
					空间->performDiscreteCollisionDetection();

					//std::list<btCollisionObject*> m_collisionObjects;
					int numManifolds = 空间->getDispatcher()->getNumManifolds();

					for (int i = 0; i < numManifolds; i++)
					{
						btPersistentManifold* contactManifold = 空间->getDispatcher()->getManifoldByIndexInternal(i);
						btCollisionObject* obA = (btCollisionObject*)(contactManifold->getBody0());
						btCollisionObject* obB = (btCollisionObject*)(contactManifold->getBody1());

						S_碰撞信息 info = {0};

						info.物体A = obA->getUserPointer();
						info.物体B = obB->getUserPointer();

						int numContacts = contactManifold->getNumContacts();
						for (int j = 0; j < numContacts; j++) {
							btManifoldPoint& pt = contactManifold->getContactPoint(j);
							if (pt.getDistance() <= 0.f) {
								btVector3 posA = pt.getPositionWorldOnA();
								btVector3 posB = pt.getPositionWorldOnB();

								info.坐标A = { posA.getX(), posA.getY(), posA.getZ() };
								info.坐标B = { posB.getX(), posB.getY(), posB.getZ() };

								info.冲量 = pt.getAppliedImpulse();
								info.周期 = pt.getLifeTime();

								btRigidBody* a = static_cast<btRigidBody*>(obA);
								btRigidBody* b = static_cast<btRigidBody*>(obB);

								if (a) {
									info.速度A = a->getLinearVelocity().length();
								}
								
								if (b) {
									info.速度B = b->getLinearVelocity().length();
								}
								

								//a->getLinearFactor();
								if (m_全局碰撞回调(info)) {
									return;
								}
								//m_collisionObjects.push_back(obA);
								//m_collisionObjects.push_back(obB);
								//printf("%d A -> {%f, %f, %f}\n", i, posA.getX(), posA.getY(), posA.getZ()); // 碰撞点
								//printf("%d B -> {%f, %f, %f}\n", i, posB.getX(), posB.getY(), posB.getZ());
							}
						}
					}


				}
				
			//}
		//);


	}
	
	
}

void S_PhysicsBullet::f_取更新物体(std::vector<Mat44f>& mat, std::vector<void*>& userData) {
	uint16 num = m_Core.m_物理域空间.size();
	auto* world = m_Core.m_物理域空间.data();


	uint32 begin = 0;
	//print positions of all objects
	for (uint32 i = 0; i < num; ++i) {
		uint32 objNum = world[i]->getNumCollisionObjects();


		userData.resize(mat.size() + objNum);
		mat.resize(mat.size() + objNum);

		auto* p_Objs = userData.data();
		auto* p_Mat = mat.data();

		

		for (uint32 j = 0; j < objNum; ++j) {
			btCollisionObject* obj = world[i]->getCollisionObjectArray()[j];
			btRigidBody* body = btRigidBody::upcast(obj);
			btTransform trans;

			if (body && body->getMotionState()) {
				body->getMotionState()->getWorldTransform(trans);
			}
			else {
				trans = obj->getWorldTransform();
			}

			//printf("world pos object %d = %f,%f,%f\n", j, float(trans.getOrigin().getX()), float(trans.getOrigin().getY()), float(trans.getOrigin().getZ()));
			trans.getOpenGLMatrix((float32*)&p_Mat[begin+j]);
			p_Objs[begin + j] = body->getUserPointer();
			
		}

		begin = mat.size();
	}

}

void S_PhysicsBullet::f_取碰撞物体(std::vector<S_碰撞信息>& objs, uint32 scenekey) {
	auto scene = m_Core.m_物理域空间[scenekey];
	scene->performDiscreteCollisionDetection();

	std::list<btCollisionObject*> m_collisionObjects;
	int numManifolds = scene->getDispatcher()->getNumManifolds();

	for (int i = 0; i < numManifolds; i++) {
		btPersistentManifold* contactManifold = scene->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject* obA = (btCollisionObject*)(contactManifold->getBody0());
		btCollisionObject* obB = (btCollisionObject*)(contactManifold->getBody1());

		int numContacts = contactManifold->getNumContacts();
		for (int32 j = 0; j < numContacts; j++) {
			btManifoldPoint& pt = contactManifold->getContactPoint(j);
			if (pt.getDistance() <= 0.f) {
				m_collisionObjects.push_back(obA);
				m_collisionObjects.push_back(obB);
				btVector3 posA = pt.getPositionWorldOnA();
				btVector3 posB = pt.getPositionWorldOnB();
				//printf("%d A -> {%f, %f, %f}\n", i, posA.getX(), posA.getY(), posA.getZ()); // 碰撞点
				//printf("%d B -> {%f, %f, %f}\n", i, posB.getX(), posB.getY(), posB.getZ());
				
				pt.getLifeTime();

				S_碰撞信息 info;
				info.距离 = pt.getDistance();
				info.坐标A = { posA.x(), posA.y(), posA.z() };
				info.坐标B = { posB.x(), posB.y(), posB.z() };
				info.冲量 = pt.getAppliedImpulse();
				info.周期 = pt.getLifeTime();

				info.物体A = const_cast<void*>((void*)obA->getUserPointer());
				info.物体B = const_cast<void*>((void*)obB->getUserPointer());

				objs.push_back(info);
			}
		}
	}
}


void S_PhysicsBullet::f_取更新物体(std::vector<std::vector<Mat44f>>& mat, std::vector<std::vector<void*>>& userData) {


}

void S_PhysicsBullet::f_添加物理体(S_物理体* body, uint32 scenekey) {
	switch(body->m_Type) {
	case E_物理体类型::E_物理体_刚体:
		m_Core.m_物理域空间[scenekey]->addRigidBody(dynamic_cast<S_Bullet刚体*>(body)->m_bt刚体);
		break;
	case E_物理体类型::E_物理体_柔体:
		break;
	case E_物理体类型::E_物理体_布料:
		break;
	case E_物理体类型::E_物理体_静态:
		m_Core.m_物理域空间[scenekey]->addRigidBody(dynamic_cast<S_Bullet静态碰撞*>(body)->m_Body);
		break;
	case E_物理体类型::E_物理体_控制:
		m_Core.m_物理域空间[scenekey]->addRigidBody(dynamic_cast<S_Bullet控制*>(body)->m_Body);
		break;

	case E_物理体类型::E_物理体_车辆:
		{
			auto* car = dynamic_cast<S_Bullet车*>(body);
			if (car) {
				m_车.push_back(car);
				m_Core.m_物理域空间[scenekey]->addRigidBody(car->m_Body);
				m_Core.m_物理域空间[scenekey]->addVehicle(car->m_vehicle);
			}
		}
		break;
	}
	
}

void S_PhysicsBullet::f_移除物理体(S_物理体* body, uint32 scenekey) {
	switch (body->m_Type) {
	case E_物理体类型::E_物理体_刚体:
		m_Core.m_物理域空间[scenekey]->removeRigidBody(dynamic_cast<S_Bullet刚体*>(body)->m_bt刚体);
		break;
	case E_物理体类型::E_物理体_静态:
		m_Core.m_物理域空间[scenekey]->removeRigidBody(dynamic_cast<S_Bullet静态碰撞*>(body)->m_Body);
		break;
	}
}

void S_PhysicsBullet::f_添加物理体碰撞回调(S_物理体* body, uint32 scenekey) {
	//m_Core.m_物理域空间[scenekey]->contactTest((btRigidBody*)body->f_getBody(), );


	

}





// 碰撞检测回调
struct MyColCallBack : btCollisionWorld::ContactResultCallback
{
public:
	btScalar addSingleResult(
		btManifoldPoint& cp,
		const btCollisionObjectWrapper* colObj0Wrap,
		int partId0,
		int index0,
		const btCollisionObjectWrapper* colObj1Wrap,
		int partId1,
		int index1)
	{
		btVector3 posA = cp.getPositionWorldOnA();
		btVector3 posB = cp.getPositionWorldOnB();
		//printf("col pos for A {%f, %f, %f}\n", posA.getX(), posA.getY(), posA.getZ());
		//printf("col pos for B {%f, %f, %f}\n", posB.getX(), posB.getY(), posB.getZ());

		return btScalar(0.f);
	};
};






/*int wheelIndex = 2;
			车->m_vehicle->applyEngineForce(0.02, wheelIndex);
			车->m_vehicle->setBrake(0.0, wheelIndex);
			wheelIndex = 3;
			车->m_vehicle->applyEngineForce(0.04, wheelIndex);
			车->m_vehicle->setBrake(0.0, wheelIndex);

			wheelIndex = 0;
			车->m_vehicle->setSteeringValue(0.3, wheelIndex);
			车->m_vehicle->applyEngineForce(0.0, wheelIndex);
			wheelIndex = 1;
			车->m_vehicle->setSteeringValue(0.3, wheelIndex);
			车->m_vehicle->applyEngineForce(0.0, wheelIndex);*/





